AP_NavEKF2: switch to optflow if gps is jammed #12482

chobitsfan · 2019-10-03T07:30:58Z

When GPS is jammed, EKF fail-safe will be triggered even a optical flow sensor is functional and ek2_flow_use = 1. In following flight, a GSP jammer is used, SV did not go above 2x FS_EKF_THRESH and neither SP or SM go above FS_EKF_THRESH. But EKF fail-safe is still triggered even optical flow is working well. EKF did not switch to optical flow.

Copter is f450 frame with pixhawk4, hereflow and sf20. This is the GPS jammer used in following test

2019-10-03 11-04-34.bin.gz

After apply this PR. In following flight, a GSP jammer is used and EKF switch to optical flow. Copter loiter well. EKF switch back to GPS after GPS jammer is turned off.

2019-10-03 10-52-48.bin.gz

this PR may need #12481

RickReeser · 2019-10-04T19:40:59Z

Thanks for tagging me in #11156. This seems to handle the third failure case I described in that issue.

Still learning the code, so forgive my questions:

Where does it switch back to using GPS when it becomes available again? Is this handled by readyToUseGPS() in setAidingMode()?

ardupilot/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

Line 169 in 71f83a1

    
           bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);

Why do we check if optiflow is available if setAidingMode() has the same check? Is there a reason to stay in AID_ABSOLUTE if the position has timed out?

chobitsfan · 2019-10-05T02:18:16Z

Hi @RickReeser

Where does it switch back to using GPS when it becomes available again?

I think it is here, switch from AID_RELATIVE to AID_ABSOLUTE

ardupilot/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

Lines 187 to 188 in 71f83a1

    
           if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) { 
        
               PV_AidingMode = AID_ABSOLUTE;

Why do we check if optiflow is available if setAidingMode() has the same check? Is there a reason to stay in AID_ABSOLUTE if the position has timed out?

In current code, it did not set PV_AidingMode = AID_NONE if posAidLossCritical

ardupilot/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

Lines 252 to 254 in 9456fe4

    
           } else if (posAidLossCritical) { 
        
               // if the loss of position is critical, declare all sources of position aiding as being timed out 
        
               posTimeout = true;

I think there is a reason, but I do not know. I think it may be better to not to change original logic too much. Hi @priseborough Could you possibly take a look at this? Thank you very much

@RickReeser Would you be able to test it? I build a firmware for pixhawk2 cubeblack from this PR
arducopter_pr12482_cubeblack.zip
Thank you very much

Jaaaky · 2019-10-05T18:55:16Z

Could you please add EKF3 support too?

RickReeser · 2019-10-17T22:50:39Z

@chobitsfan I'll get a chance to test this tomorrow or next week. Does that binary include your other PR #12481? Looks like it was merged 1 day before you commented.

chobitsfan · 2019-10-18T01:19:55Z

Could you please add EKF3 support too?

@Jaaaky I think it may be better to waiting for @priseborough to review it in EKF2 first?

Does that binary include your other PR #12481?

@RickReeser Yes. thank you very much for helping

chobitsfan · 2019-12-17T08:00:27Z

hi @RickReeser may I ask how is your tests going? Is everything OK? Thank you very much.

RickReeser · 2019-12-17T18:35:01Z

Unfortunately I had to put my test on hold for a while due to some very frustrating reasons, but hopefully I should be able to do it within the next week.

I backported your PRs and will be testing them on 3.6.12.

chobitsfan · 2019-12-18T02:20:11Z

Thank you @RickReeser
I look forward to hear you test result.

RickReeser · 2020-01-06T19:29:29Z

Just letting you know I haven't forgotten, I just keep running into roablocks. Will test this as soon as I can...

Edit: Finally got to do some testing, only to discover that the jammer is burned out. Need to order a new one.

rmackay9 · 2020-01-15T08:15:13Z

I just ran across this PR again. It's still valuable and we should try and get it in soon-ish. non-GPS navigation is climbing on my to-do list.

chobitsfan · 2020-01-16T02:15:41Z

Thank you @rmackay9
It would be very helpful if @priseborough could take a look at this.

RickReeser · 2020-01-18T00:35:16Z

Finally, after much ado, I was able to test fly these changes. I backported these PRs to Copter 3.6.12, so I flew 3.6.12 official as the baseline comparison. In these tests, I slowly descended the drone into the influence of a jammer near the ground. I set FS_EKF_THRESH = 10 to make sure the pos/vel failsafe wouldn't trigger.

My results are the same as @chobitsfan's. In the official firmware, the EKF failsafes into land mode when the GPS position times out. With this PR, it switches to optiflow as intended.

Copter 3.6.12:

Copter 3.6.12 with these changes:

logs:
jamming test PR12482.zip

chobitsfan · 2020-01-18T06:25:35Z

@RickReeser Thank you so much for helping testing

Did your copter hold well after it switch from GPS to optical flow? Did it shift a little after switch back to GPS (after GPS jammer turned off)?

rmackay9 · 2020-01-22T05:20:27Z

I've tested this in the simulator and it seems fine. The steps I used to test it were:

started Copter in SITL
param load ../Tools/autotest/default_params/copter-optflow.parm
param set EK2_GPS_TYPE 0 (to re-enable the GPS)
restarted the simulator so all the parameter took effect
armed and flew around in Loiter mode
map set showsimpos 1 (to allow seeing the simulated vehicle's actual position)
param set GPS_TYPE 0 (to temporarily disable the GPS)
flew around more in Loiter and observed some drift (but not too bad)
param set GPS_TYPE 1 (to simulate the GPS recovering)

@priseborough, @tridge, what do you think about this change? it looks fairly straight forward to me and seems to work.

EDIT: one thing I noticed is that if I disabled the flow sensor by setting FLOW_TYPE = 0 while the GPS was also disabled, the vehicle's position would jump a long way off. My guess is that this is unrelated though and that same odd position is reported by the EKF when GPS_TYPE = 0 regardless of the changes in this PR. I think this odd position stems from the EKF continuing to apply the last know velocities that it got to the position. So if it was flying at 1m/s when it lost GPS it continues to apply that 1m/s to the position from then on.

priseborough · 2020-01-22T06:42:11Z

This is a fairly safe change that performs the reversion back AID_NONE earlier rather than waiting for attAidLossCritical to go true. AID_NONE is the most reliable mode to commence optical flow navigation from.

A downside is that if too high for optical flow to work reliably, we will be without navigation for longer because when in AID_NONE the GPS checking required to commence GPS navigation is stricter. I would reconsider reverting back to AID_NONE if outside range finder range.

chobitsfan · 2020-01-22T11:37:57Z

Thank you very much @priseborough

I would reconsider reverting back to AID_NONE if outside range finder range.

I will add a check for range finder and optical flow health before reverting back to AID_NONE.

…_NONE

rmackay9 · 2020-01-30T06:33:23Z

Cool! I've been looking forward to this one! I'll probably test fly this one tomorrow.

rmackay9 · 2020-01-30T06:34:53Z

@priseborough, this is the kind of extra check you were thinking of?

RickReeser · 2020-01-30T18:48:16Z

@RickReeser Thank you so much for helping testing

Did your copter hold well after it switch from GPS to optical flow? Did it shift a little after switch back to GPS (after GPS jammer turned off)?

It did twitch a bit when it switched to optiflow, but nothing scary. Log shows it was a 3-4 degree pitch and roll.

I didn't test turning the jammer off, I landed and disarmed while the jammer was still on.

chobitsfan · 2020-01-31T03:14:33Z

Thank you @RickReeser

In my tests, it slipped a little when it switched to optiflow. It also slipped a little when it switched back to GPS. But copter is under control and everything is fine.

chobitsfan · 2020-01-31T03:16:31Z

Thank you very much for helping @rmackay9

rmackay9 · 2020-02-03T09:08:47Z

I finally managed a test today and it seemed OK. It was not an exhaustive test but I did disable the GPS (using the RCx_OPTION = 65) and then re-enabled it again and there weren't any sudden movements.
https://www.youtube.com/watch?v=guhpRAzWGy8

Here is a graph of the GPS status (scale on the right) so that we can see when it's disabled and then the EKF Longitude (in red) vs GPS longitude (in green). Note that although the GPS is disabled it keeps logging it's position).

Anyway, this all seems to work so I'm tempted to merge this. I would like to hear feedback from @priseborough and/or @tridge first if possible. If not I might just merge it.

chobitsfan · 2020-02-03T11:15:05Z

Hi @rmackay9 this is great, thank you for testing it.

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

rmackay9 · 2020-02-04T01:30:49Z

I've squashed the commits together and made a small change to replace "&" with "==" (this came up on the weekly dev call) and merged, thanks!

rmackay9 · 2020-02-04T01:31:44Z

Ah, the other thing that was discussed on the dev call was whether @chobitsfan would be willing to make a similar change in EKF3. No pressure though of course! thanks again!

chobitsfan · 2020-02-04T05:38:10Z

Thank you @rmackay9
I would be happy to help

rmackay9 requested a review from priseborough October 3, 2019 23:02

rmackay9 mentioned this pull request Oct 3, 2019

Copter: do not speed up EKF failsafe if optflow works #12481

Merged

AP_NavEKF2: switch to optflow if gps is jammed

71f83a1

chobitsfan force-pushed the pr_jamming_optflow branch from f40222a to 71f83a1 Compare October 4, 2019 02:57

chobitsfan mentioned this pull request Oct 4, 2019

EKF doesn't utilize optiflow well when GPS goes bad #11156

Open

AP_NavEKF2: check rangefinder data valid before reverting back to AID…

b00ceb4

…_NONE

rmackay9 added the DevCallTopic label Feb 3, 2020

tridge requested changes Feb 4, 2020

View reviewed changes

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp Show resolved Hide resolved

CraigElder removed the DevCallTopic label Feb 4, 2020

rmackay9 closed this Feb 4, 2020

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_NavEKF2: switch to optflow if gps is jammed #12482

AP_NavEKF2: switch to optflow if gps is jammed #12482

chobitsfan commented Oct 3, 2019 •

edited

Loading

RickReeser commented Oct 4, 2019 •

edited

Loading

chobitsfan commented Oct 5, 2019

Jaaaky commented Oct 5, 2019

RickReeser commented Oct 17, 2019

chobitsfan commented Oct 18, 2019

chobitsfan commented Dec 17, 2019

RickReeser commented Dec 17, 2019

chobitsfan commented Dec 18, 2019

RickReeser commented Jan 6, 2020 •

edited

Loading

rmackay9 commented Jan 15, 2020

chobitsfan commented Jan 16, 2020

RickReeser commented Jan 18, 2020 •

edited

Loading

chobitsfan commented Jan 18, 2020

rmackay9 commented Jan 22, 2020 •

edited

Loading

priseborough commented Jan 22, 2020

chobitsfan commented Jan 22, 2020

rmackay9 commented Jan 30, 2020

rmackay9 commented Jan 30, 2020

RickReeser commented Jan 30, 2020 •

edited

Loading

chobitsfan commented Jan 31, 2020

chobitsfan commented Jan 31, 2020

rmackay9 commented Feb 3, 2020

chobitsfan commented Feb 3, 2020

rmackay9 commented Feb 4, 2020

rmackay9 commented Feb 4, 2020

chobitsfan commented Feb 4, 2020

AP_NavEKF2: switch to optflow if gps is jammed #12482

AP_NavEKF2: switch to optflow if gps is jammed #12482

Conversation

chobitsfan commented Oct 3, 2019 • edited Loading

RickReeser commented Oct 4, 2019 • edited Loading

chobitsfan commented Oct 5, 2019

Jaaaky commented Oct 5, 2019

RickReeser commented Oct 17, 2019

chobitsfan commented Oct 18, 2019

chobitsfan commented Dec 17, 2019

RickReeser commented Dec 17, 2019

chobitsfan commented Dec 18, 2019

RickReeser commented Jan 6, 2020 • edited Loading

rmackay9 commented Jan 15, 2020

chobitsfan commented Jan 16, 2020

RickReeser commented Jan 18, 2020 • edited Loading

chobitsfan commented Jan 18, 2020

rmackay9 commented Jan 22, 2020 • edited Loading

priseborough commented Jan 22, 2020

chobitsfan commented Jan 22, 2020

rmackay9 commented Jan 30, 2020

rmackay9 commented Jan 30, 2020

RickReeser commented Jan 30, 2020 • edited Loading

chobitsfan commented Jan 31, 2020

chobitsfan commented Jan 31, 2020

rmackay9 commented Feb 3, 2020

chobitsfan commented Feb 3, 2020

rmackay9 commented Feb 4, 2020

rmackay9 commented Feb 4, 2020

chobitsfan commented Feb 4, 2020

chobitsfan commented Oct 3, 2019 •

edited

Loading

RickReeser commented Oct 4, 2019 •

edited

Loading

RickReeser commented Jan 6, 2020 •

edited

Loading

RickReeser commented Jan 18, 2020 •

edited

Loading

rmackay9 commented Jan 22, 2020 •

edited

Loading

RickReeser commented Jan 30, 2020 •

edited

Loading